/* 
 * File:   NFP.h
 * Author: alrojas
 *
 * Created on 31 de marzo de 2014, 11:56
 */

#ifndef NFP_H
#define	NFP_H

#include "utils.h"

class NFP {
public:
    NFP(){
    }
    
    struct Edge{
        
        int id;
        int index;
        Point point;
        double slope;  
        
        Edge(int id, int index, Point point, double slope){
            
            this->id = id;
            this->index = index;
            this->point = point;
            this->slope = slope;
            
        }
        
    };
    
    struct ComparatorBySlope{

        bool operator()(Edge spt1,Edge spt2){ return (spt1.slope < spt2.slope); }

    } compBySlope;
        
    struct Polygon{
        
        int id;
        
        vector < Edge > edges;
        vector < Point > points;
        
        
        struct ComparatorByYAxis{

            bool operator()(Point pt1,Point pt2){ return (pt1.y < pt2.y); }

        } compByYAxis;
        
        Polygon(){
        }
        
        Polygon(int id, vector < Point > points){
            
            this->id = id;
            
            this->points = points;
            
            uint i;
            for(i = 1; i < points.size(); i++){
                Point point = points[i] - points[i - 1];
                Edge edge(id,i - 1,point,utils::AngleOfPoint(Point(0,0),point));
                edges.push_back(edge);
            }
            
            Point point = points[0] - points[points.size() - 1];
            Edge edge(id,i - 1,point,utils::AngleOfPoint(Point(0,0),point));
            edges.push_back(edge);
            
            Rotate();
            Build();
            
        }
        
        Polygon(vector < Edge > edges){
            
            this->edges = edges;

            Build();            
            Rotate();
            Build();
            
        }
        
        bool HasClockwiseOrientation(){
            
            int i,j,k;
            int count = 0;
            double z;

            if (points.size() < 3)
                return false;

            for (i = 0; i < points.size(); i++){
               j = (i + 1) % points.size();
               k = (i + 2) % points.size();
               z  = (points[j].x - points[i].x) * (points[k].y - points[j].y);
               z -= (points[j].y - points[i].y) * (points[k].x - points[j].x);
               if (z < 0)
                  count--;
               else if (z > 0)
                  count++;
            }
            if (count > 0)
               return false;
            
            return true;
            
        }
            
        void Build(){

            vector < Point > loutput;
            Point lpoint;

            loutput.push_back(edges[0].point);
            lpoint = edges[0].point;
            for(uint i = 1; i < edges.size(); i++){
                lpoint = edges[i].point + lpoint;
                loutput.push_back(lpoint);
            }

            points = loutput;

        } 
        
        void Invert(){
            
            vector < Edge > iedges;
            
            for(uint i = 0; i < edges.size(); i++){
                Point ipoint = (-1) * edges[i].point;
                Edge iedge(edges[i].id,edges[i].index,ipoint,utils::AngleOfPoint(Point(0,0),ipoint));
                iedges.push_back(iedge);
            }
            
            this->edges = iedges;
            
            Build();            
            Rotate();
            Build();

        }
        
        // Anterior
        
        double Alpha1(int i){
            
            Point v2 = edges[i].point; 
            Point v1 = edges[i - 1].point;
            if(i == 0)
                v1 = edges[edges.size() - 1].point;
            
            return (v1.x * v2.y - v1.y * v2.x);
            
        }
                
        // Siguiente
        double Alpha2(int i){
            
            Point v2 = edges[i].point; 
            Point v1 = edges[i + 1].point;
            if((i + 1) == edges.size())
                v1 = edges[0].point;
            
            return (v2.x * v1.y - v2.y * v1.x);
            
        }
        
        
        // Anterior
        /*
        bool IsTurningPoint1(int i){
            
            double alpha1 = Alpha1(i); 
            double alpha2 = Alpha1(i - 1);
            
            if(alpha1 > 0 && alpha2 < 0){
                return true;
            }
            if(alpha1 < 0 && alpha2 > 0){
                return true;
            }
            
            return false;
            
        }
        
        
        // Siguiente
        bool IsTurningPoint2(int i){
            
            double alpha1 = Alpha2(i); 
            double alpha2 = Alpha2(i + 1);
            
            if(alpha1 > 0 && alpha2 < 0){
                return true;
            }
            if(alpha1 < 0 && alpha2 > 0){
                return true;
            }
            
            return false;
            
        }
        
        bool IsTurningPoint(Edge edge){
            
            int index = -1;
            for(uint i = 0; i < edges.size(); i++){
                if(edges[i].point == edge.point){
                    index = i;
                    break;
                }
            }
            
            return IsTurningPoint1(index) != IsTurningPoint2(index);
            
        }*/
        
        bool IsTurningPoint(int i){
            
            double alpha1 = Alpha1(i); 
            double alpha2 = Alpha2(i);
            
            if(alpha1 > 0 && alpha2 < 0){
                return true;
            }
            if(alpha1 < 0 && alpha2 > 0){
                return true;
            }
            
            return false;
            
        }
        
        bool IsTurningPoint(Edge edge){
            
            int index = -1;
            for(uint i = 0; i < edges.size(); i++){
                if(edges[i].point == edge.point){
                    index = i;
                    break;
                }
            }
            
            return IsTurningPoint(index);
            
        }
        
        bool IsConvex(){
            
            for(uint i = 0; i < edges.size(); i++){
                if(IsTurningPoint(edges[i])){
                    return false;
                }
            }
            
            return true;
        }
        
        void ChangeOrientation(){
            
            reverse(this->points.begin(),this->points.end());
            
            Polygon tmp(this->id,this->points);
            
            this->edges = tmp.edges; 
            
            /*
            vector < Edge > ledges
            
            uint i;
            for(i = 1; i < points.size(); i++){
                Point point = points[i] - points[i - 1];
                Edge edge(id,i - 1,point,utils::AngleOfPoint(Point(0,0),point));
                ledges.push_back(edge);
            }
            
            Point point = points[0] - points[points.size() - 1];
            Edge edge(id,i - 1,point,utils::AngleOfPoint(Point(0,0),point));
            ledges.push_back(edge);
            
            edges = ledges;
            
            Rotate();
            Build();
             
            */ 
                    
        }
        
        void Rotate(){
            
            vector < Point > tmp = points;
            
            std::sort(tmp.begin(),tmp.end(),compByYAxis);

            int index = utils::IndexOf(points,tmp[0]);
            int nindex = index + 1;

            Point bpoint = points[index];
            Point epoint = points[nindex];
            if(nindex == points.size()){
                epoint = points[0];
            }
            
            Point pinput = epoint - bpoint;
            
            vector < Edge > tail, loutput; 
            bool flag = false;

            for(uint i = 0; i < edges.size(); i++){
                if(!flag && edges[i].point != pinput){
                    tail.push_back(edges[i]);
                }
                else{
                    loutput.push_back(edges[i]);
                    flag = true;
                }
            }

            loutput.insert(loutput.end(),tail.begin(),tail.end());
            edges = loutput;

        }
        
        uint Size(){
            return edges.size();
        }
        
        Point FindBottomLeftPoint(){
            
            vector < Point > lpoints = points;
            
            std::sort(lpoints.begin(),lpoints.end(),utils::myobject1);

            return lpoints[0];
            
        }
        
        Point FindTopRightPoint(){
            
            vector < Point > lpoints = points;
            
            std::sort(lpoints.begin(),lpoints.end(),utils::myobject2);
            
            return lpoints[0];
            
        }
        
    };
    
    int IndexOf(vector < Edge > vinput, int id, int index){
        
        for(uint i = 0; i < vinput.size(); i++){
            if(vinput[i].id == id && vinput[i].index == index) return i;
        }
        return -1;
        
    }

    void Traslation(vector < Point > & vinput, Point pinput, vector < Point > & voutput){
        
        Rect bb = boundingRect(vinput);
        
        Point t = pinput - Point(bb.x,bb.y);
        
        vector < Point > loutput;
        for(uint i = 0; i < vinput.size(); i++){
            loutput.push_back(vinput[i] + t);
        }

        voutput = loutput;
        
    }
    
    void Traslation(vector < Point > & vinput, Point pinput, Point reference, vector < Point > & voutput){
                
        Point t = pinput - reference;
        
        vector < Point > loutput;
        for(uint i = 0; i < vinput.size(); i++){
            loutput.push_back(vinput[i] + t);
        }

        voutput = loutput;
        
    }
    
    void DrawPolygon(Mat & minput, Polygon polygon, Point reference){
        
        vector < Point > vinput;
        Traslation(polygon.points,reference,vinput);
        
        vector < vector < Point > > tvector;
        tvector.push_back(vinput);
        
        drawContours(minput,tvector,0,cvScalar(255,255,255));
        
        uint i;
        for(i = 1; i < vinput.size(); i++){
            circle(minput,vinput[i],2,cvScalar(0,0,255),CV_FILLED);
            Point p(vinput[i - 1].x,vinput[i - 1].y + 10);
            //putText(minput,utils::Point2String(vinput[i],i + 1).c_str(),vinput[i],CV_FONT_HERSHEY_COMPLEX_SMALL,0.5,cvScalar(0,255,255));
            putText(minput,utils::Point2String(polygon.edges[i - 1].point,i - 1).c_str(),p,CV_FONT_HERSHEY_COMPLEX_SMALL,0.5,cvScalar(0,255,255));
        }
        
        Point p(vinput[i - 1].x,vinput[i - 1].y + 10);
        //putText(minput,utils::Point2String(vinput[i],i + 1).c_str(),vinput[i],CV_FONT_HERSHEY_COMPLEX_SMALL,0.5,cvScalar(0,255,255));
        putText(minput,utils::Point2String(polygon.edges[i - 1].point,i - 1).c_str(),p,CV_FONT_HERSHEY_COMPLEX_SMALL,0.5,cvScalar(0,255,255));
        
    }
    
    void DrawPolygon(Mat & minput, vector < Point > vinput, Point reference){
        
        Polygon p(0,vinput);
        
        DrawPolygon(minput,p,reference);
        
    }
    
    void DrawSlopeDiagram(Mat & minput, vector < Point > & vinput, string id, Point center, Scalar color){
        
        for(uint i = 0; i < vinput.size(); i++){
            Point end = center + vinput[i];

            line(minput,center,end,color);
            //circle(minput,Point(center.x + vinput[i].x,center.y + vinput[i].y),2,cvScalar(0,0,255),CV_FILLED);
            circle(minput,end,2,cvScalar(0,0,255),CV_FILLED);
            
            putText(minput,utils::Point2String(id,i + 1).c_str(),end,CV_FONT_HERSHEY_COMPLEX_SMALL,0.5,cvScalar(0,255,255));
            putText(minput,utils::Point2String(vinput[i]).c_str(),Point(end.x,end.y + 10),CV_FONT_HERSHEY_COMPLEX_SMALL,0.5,cvScalar(0,255,255));
        }
        
    }
    
    void CuninghameGreenNoFitPolygon(vector < Polygon > & vvinput, Polygon & output){

        vector < Edge > edges = vvinput[0].edges;

        edges.insert(edges.end(),vvinput[1].edges.begin(),vvinput[1].edges.end());

        std::sort(edges.begin(),edges.end(),compBySlope);
        
        output = Polygon(edges);
        
    }
    
    void CuninghameGreenNoFitPolygon(vector < vector < Point > > & vvinput, vector < Point > & output){

        vector < Point > A = vvinput[0];
        vector < Point > B = vvinput[1];
        
        Polygon pA(0,A);
        Polygon pB(1,B);
        
        if(pA.HasClockwiseOrientation()){
            pA.ChangeOrientation();
        }
        
        if(!pB.HasClockwiseOrientation()){
            pB.ChangeOrientation();
        }
        
        vector < Polygon > ps;
        ps.push_back(pA);
        ps.push_back(pB);
        
        Polygon g;
        
        CuninghameGreenNoFitPolygon(ps,g);
        
        output = g.points;
        
    }
    
    void GhoshNoFitPolygon(vector < vector < Point > > & vvinput, vector < Point > & output){
        
        vector < Point > A = vvinput[0];
        vector < Point > B = vvinput[1];
        
        Polygon pA(0,A);
        Polygon pB(1,B);

        if(pA.HasClockwiseOrientation()){
            pA.ChangeOrientation();
        }
        
        if(pB.HasClockwiseOrientation()){
            pB.ChangeOrientation();
        }
        pB.Invert();
        
        vector < Polygon > ps;
        ps.push_back(pA);
        ps.push_back(pB);
        
        Polygon g;
        
        GhoshNoFitPolygon(ps,g);
        
        output = g.points;
        
    }
    
    void GhoshNoFitPolygon(vector < Polygon > & vvinput, Polygon & output){
        
        vector < Edge > ghosh;

        vector < Edge > edges = vvinput[0].edges;

        edges.insert(edges.end(),vvinput[1].edges.begin(),vvinput[1].edges.end());

        std::sort(edges.begin(),edges.end(),compBySlope);
        
        for(uint i = 0; i < vvinput[0].Size(); i++){
            cout << vvinput[0].edges[i].index << " " << vvinput[0].IsTurningPoint(i) << endl;
        }
        
        cout << endl;
        
        for(uint i = 0; i < edges.size(); i++){
            cout << edges[i].id << " " << edges[i].index << " " << edges[i].point << " " << edges[i].slope << endl;
        }
        
        int p = IndexOf(edges,0,0);

        int i = 0;
        int dir = 1;
        
        do{
            if(edges[p].id == 0){
                if(edges[p].index == i){
                    ghosh.push_back(edges[p]);
                    if(vvinput[0].IsTurningPoint(edges[p])){
                        dir = dir * (-1);
                    }
                    i++;
                    if(i >= vvinput[0].Size()){
                        i = 0;
                    }                    
                }
            }
            else{
                Point ipoint = edges[p].point * dir;
                Edge iedge(edges[p].id,edges[p].index,ipoint,utils::AngleOfPoint(Point(0,0),ipoint));
                ghosh.push_back(iedge);
            }
            p = p + dir;
            
        }while(i != 0);
        
        output = Polygon(ghosh);
        
    }
    
    void Test(){
        
        Mat polygons0 = Mat::zeros(600,600,CV_8UC3);
        Mat polygons1 = Mat::zeros(600,600,CV_8UC3);
        Mat CG_NFP = Mat::zeros(600,600,CV_8UC3);
        Mat GHOSH_NFP = Mat::zeros(600,600,CV_8UC3);
        
        // POLYGON A
    
        vector < Point > A;
        
        int factor = 10;
        
        Point pa1(12 * factor,0 * factor);
        Point pa2(22 * factor,6 * factor);
        Point pa3(19 * factor,13 * factor);
        Point pa4(10 * factor,16 * factor);
        Point pa5(0 * factor,6 * factor);       
        Point paa(14 * factor,8 * factor);
                
        A.push_back(pa1);
        A.push_back(pa2);
        A.push_back(pa3);
        A.push_back(paa);
        A.push_back(pa4);
        A.push_back(pa5);
         
        Polygon polygonA(0,A);
        
        // POLYGON B
        
        vector < Point > B;
        
        Point pb1(3 * factor,0 * factor); 
        Point pb2(0 * factor,3 * factor);         
        Point pb3(0 * factor,9 * factor);  
        Point pb4(3 * factor,13 * factor); 
        
        B.push_back(pb1);
        B.push_back(pb2);
        B.push_back(pb3);
        B.push_back(pb4);
        
        Polygon polygonB(1,B);
        
        Polygon polygonccwB = polygonB; 
        polygonccwB.ChangeOrientation();
        
        Polygon polygonmB = polygonccwB; 
        polygonmB.Invert();
        
        cout << "A Has Clockwise Orientation: " << polygonA.HasClockwiseOrientation() << endl; 
        cout << "B Has Clockwise Orientation: " << polygonB.HasClockwiseOrientation() << endl;
        cout << "ccwB Has Clockwise Orientation: " << polygonccwB.HasClockwiseOrientation() << endl;
        cout << "-B Has Clockwise Orientation: " << polygonmB.HasClockwiseOrientation() << endl;
        cout << endl;
        cout << "A is Convex: " << polygonA.IsConvex() << endl; 
        cout << "B is Convex: " << polygonB.IsConvex() << endl;
        cout << "ccwB is Convex: " << polygonccwB.IsConvex() << endl;
        cout << "-B is Convex: " << polygonmB.IsConvex() << endl;        
        cout << endl;
        
        DrawPolygon(polygons0,polygonA,Point(20,20));
        DrawPolygon(polygons0,polygonB,Point(400,20));
        DrawPolygon(polygons0,polygonccwB,Point(20,250));
        DrawPolygon(polygons0,polygonmB,Point(400,250));        
        
        imshow("Polygons-0",polygons0);
        imwrite("Polygons.JPG",polygons0);
                
        // Cuninghame-Green Method 
        
        Polygon CGNFP;
        
        vector < Polygon > vpolygons0;
        vpolygons0.push_back(polygonA);
        vpolygons0.push_back(polygonB );
        
        CuninghameGreenNoFitPolygon(vpolygons0,CGNFP);
        
        DrawPolygon(CG_NFP,CGNFP,Point(20,20));
        
        imshow("CG-NFP",CG_NFP);
        imwrite("CG-NFP.JPG",CG_NFP);
        
        // Ghosh Method

        Polygon GhoshNFP;
        
        vector < Polygon > vpolygons1;
        vpolygons1.push_back(polygonA);
        vpolygons1.push_back(polygonmB);
        
        GhoshNoFitPolygon(vpolygons1,GhoshNFP);

        DrawPolygon(GHOSH_NFP,GhoshNFP,Point(20,20));
        
        imshow("Ghosh-NFP",GHOSH_NFP);
        imwrite("Ghosh-NFP.JPG",GHOSH_NFP);
         
        cvWaitKey();
        
    }
    
    

};

#endif	/* NFP_H */

